(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    114348,       3218]
NotebookOptionsPosition[    112761,       3164]
NotebookOutlinePosition[    113155,       3181]
CellTagsIndexPosition[    113112,       3178]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "AMBER", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/AMBER_Output_Test/model_test\>\"", 
     "]"}], ";"}], "\n", 
   RowBox[{"<<", "RobotLinks.m"}], "\n", 
   RowBox[{"<<", "Linearize.m"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"SetDirectory", "[", 
      RowBox[{
      "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
       "\"\<build_torso\>\""}], "]"}], ";"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LineNumber", " ", "=", " ", "7"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"IndexAllPre", " ", "=", " ", 
     RowBox[{"Import", "[", 
      RowBox[{"\"\<../data/IndexAll.mat\>\"", ",", "\"\<MAT\>\""}], "]"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Dimensions", "[", "IndexAllPre", "]"}], ";"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"IndexAll", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"First", "[", "IndexAllPre", "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"Dimensions", "[", "IndexAll", "]"}], ";"}], "*)"}], 
   "\[IndentingNewLine]", "\n", 
   RowBox[{"HipPosIndex", "=", 
    RowBox[{"Round", "[", 
     RowBox[{"IndexAll", "[", 
      RowBox[{"[", 
       RowBox[{"LineNumber", ",", "1"}], "]"}], "]"}], "]"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"NSslopeIndex", "=", 
    RowBox[{"Round", "[", 
     RowBox[{"IndexAll", "[", 
      RowBox[{"[", 
       RowBox[{"LineNumber", ",", "2"}], "]"}], "]"}], "]"}]}], 
   "\[IndentingNewLine]", 
   RowBox[{"TorsoIndex", "=", 
    RowBox[{"Round", "[", 
     RowBox[{"IndexAll", "[", 
      RowBox[{"[", 
       RowBox[{"LineNumber", ",", "3"}], "]"}], "]"}], "]"}]}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, 3.527268323380795*^9, {
   3.5272685755855627`*^9, 3.527268578414917*^9}, {3.527278880328299*^9, 
   3.527278880502112*^9}, {3.5274624643640203`*^9, 3.527462465746292*^9}, {
   3.5274630644826403`*^9, 3.527463064599002*^9}, {3.52746430368642*^9, 
   3.527464303863702*^9}, {3.527464363024025*^9, 3.527464363128985*^9}, {
   3.52746480541362*^9, 3.5274648068282127`*^9}, {3.527465321987301*^9, 
   3.527465322942286*^9}}],

Cell[BoxData[
 FormBox["1", TraditionalForm]], "Output",
 CellChangeTimes->{3.527268578845785*^9, 3.527278884161552*^9, 
  3.5272803669441032`*^9, 3.5272819340319977`*^9, 3.527462467451034*^9, 
  3.52746250585408*^9, 3.5274630696636667`*^9, 3.527463137932159*^9, 
  3.52746410257344*^9, 3.527464322022376*^9, 3.527464369948999*^9, 
  3.5274648232047167`*^9, 3.527465326306275*^9}],

Cell[BoxData[
 FormBox["1", TraditionalForm]], "Output",
 CellChangeTimes->{3.527268578845785*^9, 3.527278884161552*^9, 
  3.5272803669441032`*^9, 3.5272819340319977`*^9, 3.527462467451034*^9, 
  3.52746250585408*^9, 3.5274630696636667`*^9, 3.527463137932159*^9, 
  3.52746410257344*^9, 3.527464322022376*^9, 3.527464369948999*^9, 
  3.5274648232047167`*^9, 3.527465326309565*^9}],

Cell[BoxData[
 FormBox["7", TraditionalForm]], "Output",
 CellChangeTimes->{3.527268578845785*^9, 3.527278884161552*^9, 
  3.5272803669441032`*^9, 3.5272819340319977`*^9, 3.527462467451034*^9, 
  3.52746250585408*^9, 3.5274630696636667`*^9, 3.527463137932159*^9, 
  3.52746410257344*^9, 3.527464322022376*^9, 3.527464369948999*^9, 
  3.5274648232047167`*^9, 3.527465326311769*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"t0", "=", 
     RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"model", " ", "specification"}], "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"robotInfo", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "Lc", ",", "Lt", ",", "LT", ",", "rc", ",", "rt", ",", "rT", ",", 
         "mh", ",", "mt", ",", "mc", ",", "g"}], "}"}], ",", 
       "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{
        "347.98", ",", "261.112", ",", "0", ",", "282.37", ",", "128.15", ",",
          "9.97", ",", 
         StyleBox["804.83",
          FontWeight->"Plain"], ",", "606.15", ",", "213.79", ",", "9810"}], 
        "}"}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"constsubs", "=", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"#1", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}],
          "\[Rule]", 
         RowBox[{"Rationalize", "[", 
          FractionBox[
           RowBox[{
           "#1", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
           "1000"], "]"}]}], "&"}], ")"}], "/@", 
      RowBox[{"Transpose", "[", "robotInfo", "]"}]}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"ndof", " ", "=", " ", "5"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"mm", "=", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"statesubs", "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
         RowBox[{"x", "[", "i", "]"}]}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "i"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
         RowBox[{"x", "[", 
          RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
   ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"define", " ", "inertia", " ", "matrices"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"For", "[", 
    RowBox[{
     RowBox[{"i", "=", "1"}], ",", 
     RowBox[{"i", "\[LessEqual]", 
      RowBox[{"Length", "[", "mm", "]"}]}], ",", 
     RowBox[{"i", "++"}], ",", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
      RowBox[{
       RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
       " ", 
       RowBox[{"DiagonalMatrix", "[", 
        RowBox[{"{", 
         RowBox[{"1", ",", "1", ",", "1"}], "}"}], "]"}]}]}]}], "]"}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"add", " ", "inertia", " ", "for", " ", "torso", " ", 
     RowBox[{"(", 
      RowBox[{"2.5", " ", "cm", " ", "radius"}], ")"}]}], ",", 
    RowBox[{"the", " ", "x"}], ",", 
    RowBox[{
    "z", " ", "terms", " ", "will", " ", "not", " ", "show", " ", "up", " ", 
     "in", " ", "the", " ", "EOM", " ", "for", " ", "this", " ", "2", "d", 
     " ", "model"}]}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", "12.37"},
         {"0", "1946798.09", 
          RowBox[{"-", "19.96"}]},
         {"12.37", 
          RowBox[{"-", "19.96"}], "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", "58.80", "2024.23"},
         {"58.80", "6396011.40", "146697.91"},
         {"2024.23", "146697.91", "418371.07"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"3730232.81", 
          RowBox[{"-", "7.44"}], "1948.96"},
         {
          RowBox[{"-", "7.44"}], "518271.83", 
          RowBox[{"-", "1348.28"}]},
         {"1948.96", 
          RowBox[{"-", "1348.28"}], "3577190.08"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", 
          RowBox[{"-", "60.11"}], "1985.63"},
         {
          RowBox[{"-", "60.11"}], "6396011.38", 
          RowBox[{"-", "146697.91"}]},
         {"1985.63", 
          RowBox[{"-", "146697.91"}], "418371.06"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", 
          RowBox[{"-", "12.37"}]},
         {"0", "1946798.091", "19.96"},
         {
          RowBox[{"-", "12.37"}], "19.96", "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", "5"}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", 
        SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", "1"}],
      "]"}]}]}], "]"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"p0", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}], 
      ",", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}]}], 
     "}"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"generalized", " ", "coordinates"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"q", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"dq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ddq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"qe", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", "q"}], 
    "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"dqe", "=", 
    RowBox[{
     SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
   "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
   "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "0"], "=", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "px"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "pz"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q1"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q2"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q3"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q4"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["\[Xi]", "q5"], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"RevoluteTwist", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"base", " ", "configuration"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         FractionBox[
          RowBox[{"-", "33"}], "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{
         FractionBox["2", "100000"], ",", 
         FractionBox[
          RowBox[{"-", "381"}], "100000"], ",", 
         RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", 
      SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{
          FractionBox[
           RowBox[{"-", "24"}], "100000"], ",", 
          FractionBox[
           RowBox[{"-", "4"}], "100000"], ",", 
          RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", 
      SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{
          FractionBox[
           RowBox[{"-", "24"}], "100000"], ",", 
          FractionBox[
           RowBox[{"-", "4"}], "100000"], ",", 
          RowBox[{"Lc", "+", "Lt", "+", "Lb"}]}], "}"}]}], "]"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{
         FractionBox["2", "100000"], ",", 
         FractionBox["381", "100000"], ",", 
         RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         FractionBox["33", "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.5272808449935427`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "1"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "2"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "3"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "4"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "5"], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.527280828414195*^9}, 
   3.5272808762225933`*^9, {3.527281589348708*^9, 3.52728158963411*^9}, {
   3.5272816841443167`*^9, 3.5272817411589403`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "calculate", " ", "the", " ", "forward", " ", "kinematics", " ", "maps"}], 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q5"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.527280828414195*^9}, 
   3.5272808762225933`*^9, {3.527281589348708*^9, 3.52728158963411*^9}, {
   3.5272816841443167`*^9, 3.527281777527067*^9}, {3.527464339252104*^9, 
   3.5274643454060507`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";", 
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.527280828414195*^9}, 
   3.5272808762225933`*^9, {3.527281589348708*^9, 3.52728158963411*^9}, {
   3.5272816841443167`*^9, 3.5272817333067636`*^9}, {3.527464094660346*^9, 
   3.527464095488654*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
      "constsubs"}], "]"}]}], ";", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
      "constsubs"}], "]"}]}], ";"}], "\n", 
  RowBox[{"(*", "aniplot", "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"pos", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"1", ",", "3"}], "}"}], ",", 
        RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
     "]"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.5272687345158367`*^9}, 
   3.527271522279252*^9, {3.527280823762142*^9, 3.527280828414195*^9}, 
   3.5272808762225933`*^9, {3.527281589348708*^9, 3.52728158963411*^9}, 
   3.5272816841443167`*^9, 3.527462750432034*^9}],

Cell[BoxData[
 RowBox[{"\n", 
  RowBox[{"(*", 
   RowBox[{"calculate", " ", "center", " ", "of", " ", "mass"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
           
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{"Total", "[", "mm", "]"}], ")"}]}]}]}], ")"}], "/.", 
        "constsubs"}], "//.", "p0"}], "//", "Simplify"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "manipulator", " ", "inertia", " ", 
     "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["\[ScriptCapitalJ]", "i"], "\[Transpose]"}], ".", 
        SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
        SubscriptBox["\[ScriptCapitalJ]", "i"]}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "project", " ", "out", " ", "the", " ", "generalized", " ", "coordinates",
      " ", "defining", " ", "the", " ", "position", " ", "of", " ", "the", 
     " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ", "the", " ", 
     "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"3", ";;", "All"}], ",", 
         RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[ScriptCapitalC]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"InertiaToCoriolis", "[", 
       RowBox[{"\[ScriptCapitalD]", ",", 
        RowBox[{"Flatten", "[", "q", "]"}], ",", 
        RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", " ",
      "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"V", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"g", " ", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
     "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
   "\n", 
   RowBox[{"(*", 
    RowBox[{"the", " ", "position", " ", "of", " ", "swing", " ", "foot"}], 
    "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
        "\[RightDoubleBracket]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"\[ScriptCapitalE]", "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".293"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
       ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".210"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
        ".923"}]}], "}"}]}], "*)"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.527268740044486*^9}, {
   3.527281326545467*^9, 3.527281336017424*^9}, 3.527281656208846*^9, {
   3.52746259579731*^9, 3.5274626020720882`*^9}, 3.527462638516562*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"h", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"hdot", " ", "=", "  ", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"hip", " ", "position", " ", "related"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"1", " ", "Hip", " ", "Position"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"HipPos", " ", "=", " ", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
   ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"2", " ", "Linearized", " ", "Hip", " ", "Position"}], 
   "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"LHipPos", " ", "=", " ", 
    RowBox[{"Linearize", "[", 
     RowBox[{"HipPos", ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"non", "-", 
    RowBox[{"stance", " ", "slope", " ", "related"}]}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"1", " ", "non"}], "-", 
    RowBox[{"stance", " ", "slope"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"nsslope", " ", "=", " ", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", "p0"}]}],
    ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"2.", " ", "linearized", " ", "non"}], "-", 
    RowBox[{"stance", " ", "slope"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"LinearNSslope", "=", 
    RowBox[{"Linearize", "[", 
     RowBox[{
      RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"3.", " ", "hip", " ", "angle"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"HipAngle", " ", "=", " ", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"torso", " ", "related"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
    RowBox[{"stance", " ", "thigh", " ", "angle"}]}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"theta4", " ", "=", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"TorsoHipAngle", "=", " ", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
    RowBox[{"stance", " ", "Slope"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"nstorso", " ", "=", " ", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", "p0"}]}],
    ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
    RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"LinearNStorso", "=", 
    RowBox[{"Linearize", "[", 
     RowBox[{
      RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"storso", " ", "=", " ", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]], "/.", "p0"}]}], 
   ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
   "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
   "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"LinearStorso", "=", 
    RowBox[{"Linearize", "[", 
     RowBox[{
      RowBox[{"storso", " ", "/.", "p0"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"7", " ", "COM"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["p", "COM"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        UnderoverscriptBox["\[Sum]", 
         RowBox[{"i", "=", "1"}], 
         RowBox[{"Length", "[", "mm", "]"}]], 
        RowBox[{
         RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"2", "*", 
             RowBox[{"(", 
              RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}], "/.",
        "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"8", " ", "Linearized", " ", "COM"}], "*)"}], 
  RowBox[{
   RowBox[{
    SubscriptBox["Linearp", "COM"], "=", " ", 
    RowBox[{"Linearize", "[", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "COM"], "/.", "p0"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"Subscript", "[", 
          RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], 
   ";"}]}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.527268740044486*^9}, {
   3.527281326545467*^9, 3.527281336017424*^9}, 3.527282982629828*^9, 
   3.5274626616053553`*^9, {3.52746423172756*^9, 3.5274642392924633`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"feedback", " ", "control"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"\[Chi]", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"d\[Chi]", "=", 
     RowBox[{"D", "[", 
      RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"hipOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"HipPos", ",", "LHipPos"}], "}"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hip"], " ", "=", 
     RowBox[{"hipOutput", "[", 
      RowBox[{"[", "HipPosIndex", "]"}], "]"}]}], " ", ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hipdot"], "  ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["v", "hip"], "=", 
     RowBox[{
      RowBox[{"D", "[", 
       RowBox[{
        SubscriptBox["p", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"fb", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         SubscriptBox["p", "hip"], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"time", "-", 
      RowBox[{"invariant", " ", 
       RowBox[{"parameterization", ":"}]}]}], "*)"}], "\n", 
    RowBox[{"\[Sigma]y", "=", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
       RowBox[{"Exp", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "4"}], "]"}], 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
      RowBox[{"a", "[", 
       RowBox[{"i", ",", "5"}], "]"}]}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"calculate", 
      FormBox[
       SubscriptBox["y", "d"],
       TraditionalForm], "and", " ", "its", " ", "derivatives"}], "*)"}], 
    "\n", "\n", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "1"}]], " ", "=", " ", 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
      "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "actual", " ", "kinematics", " ", "outputs", " ", "and",
      " ", "Jacobians"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["p", "hip"], ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"NSslopeOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], "}"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"TorsoOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
      "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
       ",", "storso", ",", "LinearStorso", ",", 
       SubscriptBox["p", "COM"], ",", 
       SubscriptBox["Linearp", "COM"]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"NSslopeOutput", "[", 
            RowBox[{"[", "NSslopeIndex", "]"}], "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"TorsoOutput", " ", "[", 
            RowBox[{"[", "TorsoIndex", "]"}], "]"}], "}"}]}], "}"}], "/.", 
        "constsubs"}], "/.", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], 
    ";"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268390320581*^9, 3.527268399102364*^9}, 
   3.5273682847658978`*^9, {3.527462649285791*^9, 3.52746264983113*^9}, 
   3.527462781739599*^9, {3.527462947957477*^9, 3.527462952839796*^9}, 
   3.527463028274949*^9, {3.527464296099502*^9, 3.527464297805262*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Dimensions", "[", 
  SubscriptBox["Dy", 
   RowBox[{"a", ",", "1"}]], "]"}]], "Input",
 CellChangeTimes->{{3.527463280245996*^9, 3.52746337143116*^9}, {
  3.527463407092326*^9, 3.5274634258297043`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", "10", "}"}], TraditionalForm]], "Output",
 CellChangeTimes->{{3.527463293606145*^9, 3.527463371982875*^9}, {
   3.527463409223051*^9, 3.52746342617914*^9}, 3.527464118463605*^9, 
   3.527464338506463*^9, 3.5274643858281727`*^9, 3.5274648389090843`*^9, 
   3.5274653461604233`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["Dy", 
   RowBox[{"a", ",", "1"}]], "[", 
  RowBox[{"[", 
   RowBox[{"1", ";;", "5"}], "]"}], "]"}]], "Input",
 CellChangeTimes->{{3.527463436279295*^9, 3.527463439624174*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{"-", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"-", 
          RowBox[{"685", " ", 
           RowBox[{
            SuperscriptBox[
             SubscriptBox["\[Theta]", "1"], "\[Prime]",
             MultilineFunction->None], "(", "t", ")"}], " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}]}], "-", 
         RowBox[{"514", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "1"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}], "+", 
            RowBox[{
             SuperscriptBox[
              SubscriptBox["\[Theta]", "2"], "\[Prime]",
              MultilineFunction->None], "(", "t", ")"}]}], ")"}], " ", 
          RowBox[{"sin", "(", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
        ")"}]}], "250000"]}], ",", 
    FractionBox[
     RowBox[{"32639", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "1"], "\[Prime]",
          MultilineFunction->None], "(", "t", ")"}], "+", 
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "2"], "\[Prime]",
          MultilineFunction->None], "(", "t", ")"}]}], ")"}], " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "125000"],
     ",", "0", ",", "0", ",", "0"}], "}"}], TraditionalForm]], "Output",
 CellChangeTimes->{3.5274634402339983`*^9, 3.527464118580825*^9, 
  3.527464338587994*^9, 3.5274643858821373`*^9, 3.5274648389653187`*^9, 
  3.527465346217928*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Dimensions", "[", 
  SubscriptBox["Dy", 
   RowBox[{"a", ",", "2"}]], "]"}]], "Input",
 CellChangeTimes->{{3.527463461255437*^9, 3.527463464741624*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{"4", ",", "10"}], "}"}], TraditionalForm]], "Output",
 CellChangeTimes->{3.527463465509872*^9, 3.52746411870745*^9, 
  3.5274643386919317`*^9, 3.527464385950891*^9, 3.52746483902045*^9, 
  3.5274653462886143`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["Dy", 
   RowBox[{"a", ",", "2"}]], "[", 
  RowBox[{"[", 
   RowBox[{
    RowBox[{"1", ";;", "4"}], ",", 
    RowBox[{"1", ";;", "5"}]}], "]"}], "]"}]], "Input",
 CellChangeTimes->{{3.527463447257831*^9, 3.527463469136547*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"-", 
       FractionBox[
        RowBox[{
         RowBox[{"704180", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
         "733421"}], 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}], "2"]]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{
         RowBox[{"704180", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
         "733421"}], 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}], "2"]]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{
         RowBox[{"704180", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
         "733421"}], 
        SuperscriptBox[
         RowBox[{"(", 
          RowBox[{
           RowBox[{"514", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
           RowBox[{"685", " ", 
            RowBox[{"cos", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}], "2"]]}], 
      FractionBox[
       RowBox[{
        RowBox[{"704180", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
        "733421"}], 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}], "2"]], 
      FractionBox[
       RowBox[{"685", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "(", "t", ")"}], ")"}]}], "+", 
          "685"}], ")"}]}], 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}], "2"]]},
     {"0", "1", "0", "0", "0"},
     {"0", "0", "0", "0", "1"},
     {
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "41834171195"}], " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
        RowBox[{"25096253337", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"401207755", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"6820902539", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"701338095", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"606150", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"9657960", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"606150", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
       "122235500000"], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "25096253337"}], " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"401207755", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"6820902539", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"701338095", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"606150", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"9657960", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"606150", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
       "122235500000"], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "401207755"}], " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"6820902539", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"701338095", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"9657960", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"606150", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
       "122235500000"], 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "6820902539"}], " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"701338095", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"606150", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}]}], 
       "122235500000"], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"140267619", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}], 
        "24447100000"]}]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.5274634542917852`*^9, 3.527463469781196*^9}, 
   3.5274641188308163`*^9, 3.527464285425186*^9, 3.527464338779718*^9, 
   3.527464385994948*^9, 3.527464839062559*^9, 3.527465346307558*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"a", ",", "2"}]], "[", 
   RowBox[{"[", 
    RowBox[{"4", ",", "3"}], "]"}], "]"}], "+", 
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"a", ",", "2"}]], "[", 
   RowBox[{"[", 
    RowBox[{"4", ",", "4"}], "]"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.5274652693678417`*^9, 3.5274652814110727`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{
   FractionBox[
    RowBox[{
     RowBox[{"9657960", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"606150", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"401207755", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"6820902539", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
     RowBox[{"701338095", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
    "122235500000"], "+", 
   FractionBox[
    RowBox[{
     RowBox[{"606150", " ", 
      RowBox[{"sin", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"6820902539", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
     RowBox[{"701338095", " ", 
      RowBox[{"cos", "(", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
    "122235500000"]}], TraditionalForm]], "Output",
 CellChangeTimes->{3.5274652819279757`*^9, 3.527465346366275*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"nsslope", "-", "nstorso"}], "//", "FullSimplify"}]], "Input",
 CellChangeTimes->{{3.527464153084649*^9, 3.52746417119844*^9}}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{{3.527464155075409*^9, 3.527464216317602*^9}, 
   3.527464280236889*^9, 3.527464339095194*^9, 3.527464386205173*^9, 
   3.527464839257987*^9, 3.527465346450349*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{"\[ScriptCapitalC]", "/.", "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{"\[ScriptCapitalG]", "/.", "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{"\[ScriptCapitalD]e", "/.", "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<E_mat\>\"", "]"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{"\[ScriptCapitalE]", "/.", "statesubs"}]}], "]"}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{"h", "/.", "statesubs"}]}], "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{"hdot", "/.", "statesubs"}]}], "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\[IndentingNewLine]", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"stream", "=", 
      RowBox[{"OpenWrite", "[", "\"\<deltaphip_sca\>\"", "]"}]}], ";", "\n", 
     RowBox[{"Write", "[", 
      RowBox[{"stream", ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["deltap", "hip"], "/.", "\[InvisibleSpace]", 
         "constsubs"}], "/.", "statesubs"}]}], "]"}], ";", "\n", 
     RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
     RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}], "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "hipdot"], "/.", "\[InvisibleSpace]", "constsubs"}], 
      "/.", "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"stream", "=", 
      RowBox[{"OpenWrite", "[", "\"\<deltaphip_dot_mat\>\"", "]"}]}], ";", 
     "\n", 
     RowBox[{"Write", "[", 
      RowBox[{"stream", ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["deltap", "hipdot"], " ", "/.", "\[InvisibleSpace]", 
         "constsubs"}], "/.", "statesubs"}]}], "]"}], ";", "\n", 
     RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
     RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}], "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["y", 
        RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["y", 
        RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["DLfy", 
        RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["DLfy", 
        RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["y", 
        RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["y", 
        RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["DLfy", 
        RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["DLfy", 
        RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
       "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"stream", "=", 
    RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";", "\n", 
   RowBox[{"Write", "[", 
    RowBox[{"stream", ",", 
     RowBox[{
      RowBox[{"\[Sigma]y", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
      "statesubs"}]}], "]"}], ";", "\n", 
   RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
   RowBox[{"FolderName", "=", 
    RowBox[{"\"\<build_torso_\>\"", "<>", 
     RowBox[{"ToString", "[", "LineNumber", "]"}]}]}], ";", "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
     "SetDirectory", "[", 
      "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs\>\"", "]"}], 
     ";"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
   "SetDirectory", "[", 
    "\"\</home/shu/workspace/Research/AMBER_Output_Test/model_test/\>\"", 
    "]"}], ";", "\n", 
   RowBox[{"Run", "[", "\"\<perl math2mat_torso.pl\>\"", "]"}], ";", "\n", 
   RowBox[{"Run", "[", 
    RowBox[{"\"\<mkdir -p \>\"", "<>", "FolderName"}], "]"}], ";", "\n", 
   RowBox[{"Run", "[", 
    RowBox[{
    "\"\<cp ./build_torso/*.m ./\>\"", "<>", "FolderName", "<>", 
     "\"\</\>\""}], "]"}], ";"}], "*)"}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, 3.527268390320581*^9, {3.527281377932523*^9, 
   3.52728138118757*^9}, {3.527281421988617*^9, 3.527281425495306*^9}, 
   3.527462720790564*^9, {3.527463097192411*^9, 3.527463098052043*^9}, {
   3.5274643114741087`*^9, 3.527464317954241*^9}}]
},
WindowSize->{1920, 1026},
WindowMargins->{{-10, Automatic}, {Automatic, -8}},
ShowSelection->True,
Magnification->1.100000023841858,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[567, 22, 2915, 69, 279, "Input"],
Cell[3485, 93, 380, 6, 31, "Output"],
Cell[3868, 101, 380, 6, 31, "Output"],
Cell[4251, 109, 380, 6, 31, "Output"]
}, Open  ]],
Cell[4646, 118, 2098, 51, 132, "Input"],
Cell[6747, 171, 14203, 434, 990, "Input"],
Cell[20953, 607, 8148, 239, 122, "Input"],
Cell[29104, 848, 7529, 206, 141, "Input"],
Cell[36636, 1056, 3046, 82, 127, "Input"],
Cell[39685, 1140, 7776, 221, 165, "Input"],
Cell[47464, 1363, 8242, 221, 468, "Input"],
Cell[55709, 1586, 10460, 306, 708, "Input"],
Cell[66172, 1894, 9717, 287, 575, "Input"],
Cell[CellGroupData[{
Cell[75914, 2185, 226, 5, 33, "Input"],
Cell[76143, 2192, 319, 6, 31, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[76499, 2203, 215, 6, 33, "Input"],
Cell[76717, 2211, 2051, 57, 51, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[78805, 2273, 176, 4, 33, "Input"],
Cell[78984, 2279, 264, 6, 31, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[79285, 2290, 266, 8, 33, "Input"],
Cell[79554, 2300, 17398, 461, 114, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[96989, 2766, 371, 12, 33, "Input"],
Cell[97363, 2780, 3473, 97, 92, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[100873, 2882, 161, 3, 31, "Input"],
Cell[101037, 2887, 240, 4, 31, "Output"]
}, Open  ]],
Cell[101292, 2894, 11465, 268, 1885, "Input"]
}
]
*)

(* End of internal cache information *)
